#include "Agent.h"
#include "ObjMeshManager.h"
#include "RadiansAndDegreesConverter.h"
#include "Timer.h"
#include "AISteeringBehaviour.h"
#include "AISeek.h"
#include "Game.h"
#include "CameraManager.h"
#include "AgentGroup.h"
#include "DX11TextureManager.h"

Agent::Agent()
{
	m_mass = 100;
	m_maxSpeed = 10;
	m_maxForce = 2;
	m_maxTurnVelocity = 20;
	m_oobb.centre = D3DXVECTOR3(0.0f, 0.0f, 0.0f);
	m_velocity = D3DXVECTOR3(0.0f, 0.0f, 0.0f);
	m_heading = D3DXVECTOR3(0.0f, 0.0f, 0.0f);
	m_side = D3DXVECTOR3(0.0f, 0.0f, 0.0f);
	m_force = D3DXVECTOR3(0.0f, 0.0f, 0.0f);
	m_target = nullptr;

	m_seek.SetAgent(this);
	m_seperate.SetAgent(this);

	//m_bhMap["seek"].first = &m_seek; // temp
	//m_bhMap["seek"].second = 0.5f;
	m_bhMap["seperate"].first = &m_seperate;
	m_bhMap["seperate"].second = 1.0f;

	//m_agentGroup = new AgentGroup;
	m_colour = D3DXVECTOR4(0.0f, 0.0f, 0.0f, 1.0f);
	m_maxVelocity = D3DXVECTOR3(1.0f, 1.0f, 1.0f);

//	m_mazeCellSize = TheGame::Instance()->GetMaze()->GetCellSize();
//	m_mazeSize = TheGame::Instance()->GetMaze()->GetGridSize();
//	m_mazePos = TheGame::Instance()->GetMaze()->GetPos();
	
	m_aggro = 30.0f;
	m_halfAggro = 0.0f;
	m_firstDraw = true;
}

Agent::~Agent()
{
	//delete m_target;
}

//void Agent::ChooseBehaviour()
//{
//	if (enemy = near)
//	{
//		m_sb = flee;
//	}
//	else
//	{
//		m_sb = seek;
//	}
//}

Neighbours Test()
{
	return Neighbours();
}

void Agent::Update()
{	
	D3DXVECTOR3 camPos = TheCameraManager::Instance()->GetCameraPos();
	
	camPos.x += m_mazeCellSize.x * (m_mazeSize.x * 0.5f);
	camPos.z += m_mazeCellSize.z * (m_mazeSize.y * 0.5f);

	float rad = 3.0f;

	D3DXVECTOR3 pos = m_oobb.centre;
	if (
		((pos.x + m_aggro >= camPos.x - rad && pos.x - m_aggro <= camPos.x + rad) || 
		(pos.x - m_aggro <= camPos.x + rad && pos.x + m_aggro >= camPos.x + rad)) &&
		((pos.z + m_aggro >= camPos.z - rad && pos.z - m_aggro <= camPos.z + rad) || 
		(pos.z - m_aggro <= camPos.z + rad && pos.z + m_aggro >= camPos.z + rad))
		)
	{
		UpdatePosition();
		m_colour = D3DXVECTOR4(1.0f, 0.0f, 0.0f, 1.0f);
	}
	else
	{
		m_colour = D3DXVECTOR4(0.5f, 0.0f, 0.0f, 1.0f);
	}
}

void Agent::UpdatePosition()
{
	// find neighbours
	m_neighbours = m_agentGroup->GetNeighbours(this, 70.0f);

	if (m_neighbours.size() > 1)
	{
		m_bhMap["seperate"].second = 10.0f;
	}
	else
	{
		m_bhMap["seperate"].second = 0.0f;
	}

	// combine steering behaviours
	for (AIBehaviourMap::iterator it = m_bhMap.begin(); it != m_bhMap.end(); it++)
	{
		it->second.first->Update();

		std::string name = it->first;
		AISteeringBehaviourAndWeight sbw = it->second;
		AISteeringBehaviour* sb = sbw.first;
		float weight = sbw.second;

		sb->Update();
		m_force *= sbw.second;
	}

	m_seek.Update();

	if ((D3DXVec3LengthSq(&m_force)) > m_maxForce)
	{
		D3DXVec3Normalize(&m_force, &m_force);
		m_force *= m_maxForce;
	}

	// calc acceleration
	D3DXVECTOR3 acceleration = m_force * (1.0f / m_mass); // inverse mass

	// calc velocity
	D3DXVECTOR3 oldVel = m_velocity;
	m_velocity += acceleration * TheTimer::Instance()->GetDt();
	
	// clamp velocity
	if (D3DXVec3LengthSq(&m_velocity) > D3DXVec3LengthSq(&m_maxVelocity))
	{
		D3DXVec3Normalize(&m_velocity, &m_velocity);
		D3DXVec3Mult(&m_velocity, m_maxVelocity);
	}

	// update position
	m_oobb.centre += (oldVel + m_velocity) * 0.5f * TheTimer::Instance()->GetDt();

//	TheGame::Instance()->GetMaze()->CheckCollision(m_oobb);

	// update heading and side vectors
	D3DXVec3Normalize(&m_heading, &m_velocity);
	m_heading.z = -m_heading.z;
	m_side = m_heading;
}

void Agent::Draw()
{
	D3DXVECTOR3 tPos = m_oobb.centre;
	tPos.x -= m_mazeCellSize.x * (m_mazeSize.x * 0.5f);
	tPos.z -= m_mazeCellSize.z * (m_mazeSize.y * 0.5f); 
	tPos.y = m_mazeCellSize.y + (m_mass * 0.5f);

	float rotAngle = atan2(m_heading.x, m_heading.z);
	TheObjMeshManager::Instance()->ChangeShader("agent_cube", "DefaultTexture");
	//TheObjMeshManager::Instance()->ChangeTexture("agent_cube", 
	//	TheDX11TextureManager::Instance()->UseTexture(L"../../Assets/enemy_texture.png"));
	TheObjMeshManager::Instance()->SetColour("agent_cube", m_colour);
	TheObjMeshManager::Instance()->Translate("agent_cube", tPos);
	TheObjMeshManager::Instance()->Rotate("agent_cube", -rotAngle + M_PI_2, 0, 1, 0);
	TheObjMeshManager::Instance()->Scale("agent_cube", D3DXVECTOR3(m_mass, m_mass, m_mass));
	TheObjMeshManager::Instance()->Draw("agent_cube");
	//TheObjMeshManager::Instance()->ChangeShader("agent_cube", "agent_cube_1");

	//m_oobb.Draw();
}

void Agent::DrawMiniMapElements()
{
	if (m_firstDraw)
	{	
		m_firstDraw = false;
	}
	else
	{
		//D3DXVECTOR3 pos = m_oobb.centre;
		//pos.y += m_mazeCellSize.y * 1.5f;
		
		TheObjMeshManager::Instance()->ChangeShader("maze_floor", "DefaultColour");
		TheObjMeshManager::Instance()->Scale("maze_floor", D3DXVECTOR3(1.0f, 1.0f, 1.0f));
		TheObjMeshManager::Instance()->SetColour("maze_floor", D3DXVECTOR4(1.0f, 0.0f, 0.0f, 1.0f));
		TheObjMeshManager::Instance()->Translate("maze_floor", m_mazePos);
		TheObjMeshManager::Instance()->Translate("maze_floor", m_oobb.centre);
		TheObjMeshManager::Instance()->Scale("maze_floor", m_mazeCellSize * 0.5f);
		TheObjMeshManager::Instance()->Draw("maze_floor");
		TheObjMeshManager::Instance()->SetColour("maze_floor", D3DXVECTOR4(1.0f, 1.0f, 1.0f, 1.0f));
		TheObjMeshManager::Instance()->ChangeShader("maze_floor", "maze_floor_1");
	}
}

float Agent::GetMass()
{
	return m_mass;
}

void Agent::SetMass(float mass)
{
	m_mass = mass;
	m_oobb.extents[0] = m_mass * 2.0f;
	m_oobb.extents[1] = m_mass * 2.0f;
	m_oobb.extents[2] = m_mass * 2.0f;
}

float Agent::GetMaxSpeed()
{
	return m_maxSpeed;
}

void Agent::SetMaxSpeed(float speed)
{
	m_maxSpeed = speed;
}

float Agent::GetMaxForce()
{
	return m_maxForce;
}

void Agent::SetMaxForce(float force)
{
	m_maxForce = force;
}

float Agent::GetMaxTurnVel()
{
	return m_maxTurnVelocity;
}

void Agent::SetMaxTurnVel(float turnVel)
{
	m_maxTurnVelocity = turnVel;
}

D3DXVECTOR3& Agent::GetPosition()
{
	D3DXVECTOR3 tPos = m_oobb.centre;
	tPos.x -= m_mazeCellSize.x * (m_mazeSize.x * 0.5f);
	tPos.z -= m_mazeCellSize.z * (m_mazeSize.y * 0.5f); 
	tPos.y = m_mazeCellSize.y * 1.5f;
	return m_oobb.centre;
}

void Agent::SetPosition(const D3DXVECTOR3& pos)
{
	m_oobb.centre = pos;
}

D3DXVECTOR3& Agent::GetVel()
{
	return m_velocity;
}

void Agent::SetVel(const D3DXVECTOR3& vel)
{
	m_velocity = vel;
}

D3DXVECTOR3& Agent::GetMaxVel()
{
	return m_maxVelocity;
}

void Agent::SetMaxVel(const D3DXVECTOR3& vel)
{
	m_maxVelocity = vel;
}

D3DXVECTOR3& Agent::GetHeading()
{
	return m_heading;
}

void Agent::GetHeading(const D3DXVECTOR3& heading)
{
	m_heading = heading;
}

D3DXVECTOR3& Agent::GetSide()
{
	return m_side;
}

void Agent::SetSide(const D3DXVECTOR3& side)
{
	m_side = side;
}

Agent*& Agent::GetTarget()
{
	return m_target;
}

void Agent::SetTarget(Agent* agent)
{
	m_target = agent;
	m_seek.SetTarget(agent);
	m_seperate.SetTarget(agent);
}

Neighbours& Agent::GetNeighbours()
{
	return m_neighbours;
}

void Agent::AddNeighbor(Agent* a)
{
	m_neighbours.push_back(a);
}

void Agent::SetGroup(AgentGroup* agentGroup)
{
	m_agentGroup = agentGroup;
}

D3DXVECTOR3& Agent::GetForce()
{
	return m_force;
}

void Agent::SetForce(const D3DXVECTOR3& force)
{
	m_force = force;
}

void Agent::AddForce(const D3DXVECTOR3& force)
{
	m_force += force;
}

void Agent::SetColour(const D3DXVECTOR4& col)
{
	m_colour = col;
}

void Agent::SetAggressiveness(float f)
{
	m_aggro = f;
	m_halfAggro = m_aggro * 0.5f;
}

float Agent::GetAggressiveness() const
{
	return m_aggro;
}

OOBB3& Agent::GetOOBB()
{
	return m_oobb;
}